#include "camera_select.h"
using namespace cv;
CameraSelect::CameraSelect(){
    nh_private_ = ros::NodeHandle("~");
    average_lines_ = nh_private_.param("average_line",30);
    bucket_radius_ = nh_private_.param("bucket_radius",25);
    circle_max_radius_ = nh_private_.param("circle_max_radius",25.2);
    circle_min_radius_ = nh_private_.param("circle_min_radius",24.8);
    num_ = nh_private_.param("cluster_num",100);
}
using namespace std;

void CameraSelect::poject_buc_select(cv::Mat image_point3d,cv::Mat image_depth, std::vector<std::map<int,int>>& average_dis, int n){
    cv::Mat project_disp(600,480,CV_8UC1,cv::Scalar(0));
    int key;
    int n_ = 0;
    int rows_start = 0;//240;
    int rows_end = image_depth.rows-30;// - 100;
    int rows_sum = rows_end - rows_start;
    average_dis.clear();
    average_dis.resize(n);
    vector<std::map<int,int>>::iterator it;
    vector<std::map<int,int>>::iterator it_;
    for(it = average_dis.begin();it!= average_dis.end();it++){
      key = 0;
      for(int j=0;j<image_depth.cols;j++){
        int distance_p = 0;
        int num = 0;
        key++;
        for(int i=rows_start+n_*rows_sum/n;i<(rows_start+n_*rows_sum/n)+20;i++){
                if((int)image_depth.at<ushort>(i,j)<2000&&(int)image_depth.at<ushort>(i,j)>500){
                if(image_point3d.at<short>(i,j)<600&&image_point3d.at<short>(i,j)>-500){
                    distance_p += (int)image_depth.at<ushort>(i,j);
                    num++;
                }
            }
        }
        if(num!=0){
            (*it)[key] = distance_p/num;
        }else{
            (*it)[key] = 0;
        }
     }
     n_++;
   }

//    for(it_ =average_dis.begin();it_!= average_dis.end();it_++){
//      for(int i=1;i<(*it_).size();i++){
//        int x = 400-(*it_)[i]/10;
//        int y = 0;
//        if(i<(*it_).size()/2){
//            y = 240-(*it_)[i]/10*tan(abs(320-i)*60/640*3.14/180);
//        }
//        else{
//            y = 240+(*it_)[i]/10*tan(abs(320-i)*60/640*3.14/180);
//        }
//        project_disp.at<uchar>(x,y) = 255;
//    }
//  }
//    cv::imshow("qqqq",project_disp);
}

void CameraSelect::poject_buc_select(cv::Mat depth, std::vector<std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>>& cloud_clusters_final){
    cv::Mat project_disp1(600,480,CV_8UC1,cv::Scalar(0));
    if(cloud_clusters_final.size()!=0){
        for(int i=0;i<cloud_clusters_final[0].size();i++){
            for(int j=0;j<cloud_clusters_final[0][i]->points.size();j++){
                project_disp1.at<uchar>(cloud_clusters_final[0][i]->points[j].y,cloud_clusters_final[0][i]->points[j].x) = 255;
            }
        }
    }
//    cv::circle(project_disp1,cv::Point(240,400),1,cv::Scalar(255));
//    cv::imshow("project_disp1", project_disp1);
//    cv::waitKey(1);
}

void CameraSelect::cluster_buc_select(cv::Mat depth,std::vector<std::map<int,int>>& average_dis,std::vector<std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>>& cloud_clusters_final){
    cloud_clusters_final.clear();
    std::vector<std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>> cloud_clusters_;

//    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloud_clusters;
//    cluster_buc(depth, average_dis,cloud_clusters);
//    cloud_clusters_.push_back(cloud_clusters);
//    std::cout<<"cloud_clusters::"<<cloud_clusters.size()<<std::endl;


    std::map<int,int> average_distant;
    for(int i=1;i<640;i++){
        int sum = 0;
        int num = 0;
        for(int j=0;j<average_dis.size();++j){
            if(average_dis[j][i]>0){
                sum += average_dis[j][i];
                num++;
            }
        }
        if(num!=0){
            average_distant[i] = sum/num;
        }
        else{
            average_distant[i] = 0;
        }

    }

//    cv::Mat project_disp(600,480,CV_8UC1,cv::Scalar(0));
//    std::vector<std::map<int,int>> average_distant_nb;
//    average_distant_nb.push_back(average_distant);
//    vector<std::map<int,int>>::iterator it_;
//    for(it_ =average_distant_nb.begin();it_!= average_distant_nb.end();it_++){
//      for(int i=1;i<(*it_).size();i++){
//        int x = 400-(*it_)[i]/10;
//        int y = 0;
//        if(i<(*it_).size()/2){
//            y = 240-(*it_)[i]/10*tan(abs(320-i)*60/640*3.14/180);
//        }
//        else{
//            y = 240+(*it_)[i]/10*tan(abs(320-i)*60/640*3.14/180);
//        }
//        project_disp.at<uchar>(x,y) = 255;
//      }
//    }
//    cv::imshow("adadadad",project_disp);
//    cv::waitKey(1);

    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloud_clusters;
    cluster_buc(depth,average_distant,cloud_clusters);
    cloud_clusters_.push_back(cloud_clusters);

//    for(int i=0;i<average_dis.size();i++){
//        std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloud_clusters;
//        cluster_buc(depth,average_dis[i],cloud_clusters);
//        cloud_clusters_.push_back(cloud_clusters);
//    }

    for(int i=0;i<cloud_clusters_.size();i++){
        std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloud_clusters_buc;
        find_cluster(cloud_clusters_[i],cloud_clusters_buc);
        //std::cout<<"cloud_clusters_buc"<<cloud_clusters_buc.size()<<std::endl;
        cloud_clusters_final.push_back(cloud_clusters_buc);
        //std::cout<<"cloud_clusters_final::"<<cloud_clusters_final.size()<<std::endl;
    }
}

void CameraSelect::judge_point_num(std::vector<std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>>& cloud_clusters_final){
    for(int i=0;i<cloud_clusters_final.size();){
      if(cloud_clusters_final[i].size()!=0){
        //计算点的数量
        float distance = 0;
        for(int j=0;j<cloud_clusters_final[i][0]->points.size();j++){
          distance += cloud_clusters_final[i][0]->points[j].x;
        }
        distance = distance/cloud_clusters_final.size();
        int num = (int)atan(bucket_radius_/(400-distance))*2/(3.14/3);
        //std::cout<<"num::"<<std::endl;
        if(cloud_clusters_final[i][0]->points.size()<0.9*num){
          cloud_clusters_final.erase(cloud_clusters_final.begin()+i);
        }
        else{
          break;
        }
      }
      else{
        cloud_clusters_final.erase(cloud_clusters_final.begin()+i);
      }
    }
}



void CameraSelect::judge_point_dis(std::vector<std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>>& cloud_clusters_final){
    int m = 0;
    for(int i=0;i<cloud_clusters_final.size();){

        bool flag = true;
        for(int nn=0;nn<cloud_clusters_final.size();nn++){
          //std::<<"size::"<<cloud_clusters_final[nn].size()<<std::endl;
        }
        if(cloud_clusters_final[i].size()!=0){
            for(int j=0;j<cloud_clusters_final[i][0]->points.size()-3;j++){
               if((cloud_clusters_final[i][0]->points[j].x-cloud_clusters_final[i][0]->points[j+3].x)>3||(cloud_clusters_final[i][0]->points[j].y-cloud_clusters_final[i][0]->points[j+3].y)>3){
                   cloud_clusters_final.erase(cloud_clusters_final.begin()+i);
                   m++;
                   flag = false;
                   break;
               }
            }
            if(flag != false){
               // std::cout<<"mmm:::"<<m<<std::endl;
                i++;
                break;
            }
        }
        else{
          cloud_clusters_final.erase(cloud_clusters_final.begin()+i);
        }
    }
}


void CameraSelect::cluster_buc(cv::Mat depth, std::vector<std::map<int,int>>& average_dis,std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& cloud_clusters){
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);  //存储源点云
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
    cloud->width    = average_dis.size()*640;
    cloud->height   = 1;
    cloud->is_dense = true;
    cloud->points.resize (cloud->width * cloud->height);
    for(int j=0;j<average_dis.size();++j){


    for(int i=1;i<average_dis[j].size();++i)
    {
        int x = 400-average_dis[j][i]/10;
        int y = 0;
        if(i<average_dis.size()/2){
            y = 240-average_dis[j][i]/10*tan(abs(320-i)*60/640*3.14/180);
        }
        else{
            y = 240+average_dis[j][i]/10*tan(abs(320-i)*60/640*3.14/180);
        }
       cloud->points[i].x=y;
       cloud->points[i].y=x;
       cloud->points[i].z=0;
    }
}
    tree->setInputCloud (cloud);
    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
    ec.setClusterTolerance (5); // 2cm
    ec.setMinClusterSize (10);
    ec.setMaxClusterSize (600);
    ec.setSearchMethod (tree);
    ec.setInputCloud (cloud);
    ec.extract (cluster_indices);

    for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
    {
       pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
       for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
       cloud_cluster->points.push_back (cloud->points[*pit]); //*
       cloud_cluster->width = cloud_cluster->points.size ();
       cloud_cluster->height = 1;
       cloud_cluster->is_dense = true;
       cloud_clusters.push_back(cloud_cluster);
    }
    for(int i=0;i<cloud_clusters.size();)
    {
        if(cloud_clusters[i]->points.size()<num_)
        {
          cloud_clusters.erase(cloud_clusters.begin()+i);
        }
        else
          ++i;
    }
}

void CameraSelect::cluster_buc(cv::Mat depth, std::map<int,int>& average_dis,std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& cloud_clusters){
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);  //存储源点云
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
    cloud->width    = average_dis.size();
    cloud->height   = 1;
    cloud->is_dense = true;
    cloud->points.resize (cloud->width * cloud->height);
    for(int i=1;i<average_dis.size();++i)
    {
        int x = 400-average_dis[i]/10;
        int y = 0;
        if(i<average_dis.size()/2){
            y = 240-average_dis[i]/10*tan(abs(320-i)*60/640*3.14/180);
        }
        else{
            y = 240+average_dis[i]/10*tan(abs(320-i)*60/640*3.14/180);
        }
       cloud->points[i].x=y;
       cloud->points[i].y=x;
       cloud->points[i].z=0;
    }

    tree->setInputCloud (cloud);
    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
    ec.setClusterTolerance (5); // 2cm
    ec.setMinClusterSize (10);
    ec.setMaxClusterSize (600);
    ec.setSearchMethod (tree);
    ec.setInputCloud (cloud);
    ec.extract (cluster_indices);

    for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
    {
       pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
       for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
       cloud_cluster->points.push_back (cloud->points[*pit]); //*
       cloud_cluster->width = cloud_cluster->points.size ();
       cloud_cluster->height = 1;
       cloud_cluster->is_dense = true;
       cloud_clusters.push_back(cloud_cluster);
    }
    for(int i=0;i<cloud_clusters.size();)
    {
        if(cloud_clusters[i]->points.size()<num_)
        {
          cloud_clusters.erase(cloud_clusters.begin()+i);
        }
        else
          ++i;
    }

//    cv::Mat project_disp(600,480,CV_8UC1,cv::Scalar(0));
//    for(int i=0;i<cloud_clusters.size();i++){
//        for(int j=0;j<cloud_clusters[i]->points.size();j++){
//            project_disp.at<uchar>(cloud_clusters[i]->points[j].y,cloud_clusters[i]->points[j].x) = 255;
//        }
//    }
//    cv::imshow("zzz",project_disp);
//    cv::waitKey(1);
}

void CameraSelect::find_cluster(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloud_clusters,std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& cloud_clusters_buc){
    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloud_clusters1;
    for(int i=0;i<cloud_clusters.size();++i){
        if(cloud_clusters[i]->points[0].x<240&&cloud_clusters[i]->points[cloud_clusters[i]->points.size()-1].x>240){
            cloud_clusters_buc.push_back(cloud_clusters[i]);
            break;
        }
    }
//    if(cloud_clusters_buc.size() == 0){
//        std::vector<int> points_num;
//        for(int i=0;i<cloud_clusters.size();i++){
//            points_num.push_back(cloud_clusters[i]->points.size());
//        }
//        sort(points_num.begin(),points_num.end());
//        for(int i=0;i<cloud_clusters.size();i++){
//            std::cout<<"num::"<<points_num[0]<<std::endl;
//            if(cloud_clusters[i]->points.size()==points_num[0]&&points_num[0]>20){
//               cloud_clusters_buc.push_back(cloud_clusters[i]);
//            }
//        }
//    }
}

void CameraSelect::distance_buc(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& cloud_clusters_buc,geometry_msgs::Pose& bucket2camera){
    std::vector<cv::Point2f> points;
    std::vector<float> points_y;
      if(cloud_clusters_buc.size()!=0){
      for(int i=0;i<cloud_clusters_buc[0]->points.size()-4;i++){
          cv::Point2f point(0,0);
          for(int j=i;j<i+3;j++){
              point.x += cloud_clusters_buc[0]->points[j].x;
              point.y += cloud_clusters_buc[0]->points[j].y;
          }
          point.x = point.x/3;
          point.y = point.y/3;
          points_y.push_back(point.y);
          points.push_back(point);
      }
      sort(points_y.begin(),points_y.end());
      for(int i=0;i<points.size();i++){
          if(points[i].y==points_y[points_y.size()-1]){
              bucket2camera.position.x = 400-points[i].y;
              bucket2camera.position.y = 240-points[i].x;
              bucket2camera.position.z = 0;
              //std::cout<<"pointx::"<<240-points[i].x<<" pointy::"<<400-points[i].y<<std::endl;
              break;
          }
      }
    }
}



void CameraSelect::camera_select(cv::Mat image_point3d, cv::Mat disp,geometry_msgs::Pose& bucket2camera,geometry_msgs::Pose& circle_center_point){
    vector<Eigen::VectorXf> model_circles;
    std::map<int,int> average_dis;
    std::vector<std::map<int,int>> average_dis_1;
    int n = 6;
    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloud_clusters;
    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloud_clusters_buc;
    std::vector<std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>> cloud_clusters_final;
    poject_buc_select(image_point3d,disp,average_dis_1,n);
    cluster_buc_select(disp,average_dis_1,cloud_clusters_final);
    //cluster_buc(disp, average_dis_1,cloud_clusters);
    judge_point_num(cloud_clusters_final);
//    judge_point_dis(cloud_clusters_final);
    poject_buc_select(disp,cloud_clusters_final);
    if(cloud_clusters_final.size()!=0){
        distance_buc(cloud_clusters_final[0],bucket2camera);
        //std::cout<<"cloud_clusters_final[0].size()::"<<cloud_clusters_final[0].size()<<std::endl;
        if(cloud_clusters_final[0].size()!=0){
            circle_select(cloud_clusters_final[0],model_circles);
            //cout<<"model_circles.size()::"<<model_circles.size()<<endl;
            cv::Mat cluster_circle(600,480,CV_32FC3,cv::Scalar(0,0,0));

            for(int i=0;i<cloud_clusters_final[0].size();i++){
               for(int j=0;j<cloud_clusters_final[0][i]->points.size();j++){
                   if(cloud_clusters_final[0][i]->points[j].y>0&&cloud_clusters_final[0][i]->points[j].y<600&&
                           cloud_clusters_final[0][i]->points[j].x>0&&cloud_clusters_final[0][i]->points[j].x<480){
                       cluster_circle.at<cv::Vec3f>(cloud_clusters_final[0][i]->points[j].y,cloud_clusters_final[0][i]->points[j].x)[0] = 255;
                       cluster_circle.at<cv::Vec3f>(cloud_clusters_final[0][i]->points[j].y,cloud_clusters_final[0][i]->points[j].x)[1] = 255;
                       cluster_circle.at<cv::Vec3f>(cloud_clusters_final[0][i]->points[j].y,cloud_clusters_final[0][i]->points[j].x)[2] = 255;
                   }
               }
            }
//            for(int i=0;i<model_circles.size();i++){
//                if(model_circles[i][0]>0&&model_circles[i][0]<600&&
//                        model_circles[i][1]>0&&model_circles[i][1]<480){
//                    cv::circle(cluster_circle, cv::Point(model_circles[i][0],model_circles[i][1]),model_circles[i][2], CV_RGB(0, 255, 255), 1, 8, 0);
//                    cout<<"circle:("<<400-model_circles[i][1]<<","<<240-model_circles[i][0]<<")"<<endl;
//                }
//            }
//            cv::imshow("model_circle", cluster_circle);
//            cv::waitKey(1);
        }

    }
    mix_circle(circle_center_point);

//        Mat Map2D_select1(1000,752, CV_32FC3,Scalar(0,0,0));
//        for(int i=0;i<cloud_clusters.size();i++)
//        {
//          for(int j=0;j<cloud_clusters[i]->points.size();j++)
//          {
//            Map2D_select1.at<Vec3f>(cloud_clusters[i]->points[j].y,cloud_clusters[i]->points[j].x)[0]=255;
//            Map2D_select1.at<Vec3f>(cloud_clusters[i]->points[j].y,cloud_clusters[i]->points[j].x)[1]=255;
//            Map2D_select1.at<Vec3f>(cloud_clusters[i]->points[j].y,cloud_clusters[i]->points[j].x)[2]=255;
//          }
//          circle(model_circle, cv::Point(model_circles[i][0],model_circles[i][1]),model_circles[i][2], CV_RGB(0, 255, 255), 1, 8, 0);
//        }
//        imshow("",Map2D_select1);
//        waitKey(10);
}


void CameraSelect::circle_select(vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& cloud_clusters,vector<Eigen::VectorXf> &model_circles){
  for(int i=0;i<cloud_clusters.size();i++)
  {
    vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> final_circles;
    pcl::PointCloud<pcl::PointXYZ>::Ptr final_circle (new pcl::PointCloud<pcl::PointXYZ>);
    std::vector<int> inliers_circle;
    Eigen::VectorXf model_coefficients;
    pcl::SampleConsensusModelCircle2D<pcl::PointXYZ>::Ptr
            model_circle (new pcl::SampleConsensusModelCircle2D<pcl::PointXYZ> (cloud_clusters[i]));
    pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_circle);
    model_circle->radius_min_=circle_min_radius_;
    model_circle->radius_max_=circle_max_radius_;
    ransac.setDistanceThreshold (2);    //与平面距离小于2的点称为局内点考虑
    ransac.computeModel();                   //执行随机参数估计
    ransac.getModelCoefficients(model_coefficients);
    ransac.getInliers(inliers_circle);                 //存储估计所得的局内点
    pcl::copyPointCloud<pcl::PointXYZ>(*cloud_clusters[i], inliers_circle, *final_circle);
    final_circles.push_back(final_circle);
    model_circles.push_back(model_coefficients);
  }
  model_circles_series_.push_back(model_circles);
}

void CameraSelect::mix_circle(geometry_msgs::Pose& circle_center_point){
    //std::cout<<model_circles_series_.size()<<std::endl;
    if(model_circles_series_.size()>10){
        float average_x = 0;
        float average_y = 0;
        int num = 0;
        for(int i=model_circles_series_.size()-10;i<model_circles_series_.size();i++){
            if(model_circles_series_[i].size()!=0){
                average_x += model_circles_series_[i][0][0];
                average_y += model_circles_series_[i][0][1];
                num++;
            }
        }
        average_x = average_x/num;
        average_y = average_y/num;
        float average_x_sec = 0;
        float average_y_sec = 0;
        int num_sec = 0;
        for(int i=model_circles_series_.size()-10;i<model_circles_series_.size();i++){
            if((model_circles_series_[i][0][0]-average_x)*(model_circles_series_[i][0][0]-average_x)
                    +(model_circles_series_[i][0][1]-average_y)*(model_circles_series_[i][0][1]-average_y)<bucket_radius_*bucket_radius_){
                average_x_sec += model_circles_series_[i][0][0];
                average_y_sec += model_circles_series_[i][0][1];
                num_sec++;
            }
        }

        average_x_sec = average_x_sec/num_sec;
        average_y_sec = average_y_sec/num_sec;

        circle_center_point.position.x = 400-average_y_sec;
        circle_center_point.position.y = 240-average_x_sec;
//        for(int i=model_circles_series.size()-10;i<model_circles_series.size();i++){
//            if(model_circles_series[i].size()!=0){
//            }
//        }
    }
//    if(model_circles.size()!=0){

//    }
}
